/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "combos.h"



void vehicelCmdInit(combos_s* combos, const vehicleCtrlCmdNew_s* cmd, uint8_t cmdCount, uint8_t cycleTimes)
{
    combos->cmd = (vehicleCtrlCmdNew_s*)cmd;
    combos->cmdUsed = cmdCount;
    combos->executeTimes = cycleTimes;
    combos->executedTimes = 0;
    combos->CurrentCmd = 0;
    combos->state = COMBOS_PAUSE;
    combos->timestamp = millis();
}
void vehicleCmdReset(combos_s* combos)
{
    combos->CurrentCmd = 0;
    combos->executedTimes = 0;
    combos->firedTime = 0;
    combos->movedTime = 0;

}
extern void callProheadingstartupdate();
void vehicelCmdRun(combos_s* combos)
{
    if (millis() > (combos->timestamp + 10)) //10ms更新一次
    {
        combos->timestamp = millis();

        if (combos->state == COMBOS_STOP)
        {
            if (combos->state != combos->statePre)
            {
                combos->statePre = combos->state;
                // vehicleCmdReset(combos);
                program_vehicleWheelCtrl(0, 0, 0);
                program_gunturretCtrl(0, 0);
                // combos->state = COMBOS_PAUSE;
            }
        }
        if (combos->state == COMBOS_PAUSE)
        {
            if (combos->state != combos->statePre)
            {
                combos->statePre = combos->state;
                program_vehicleWheelCtrl(0, 0, 0);
                program_gunturretCtrl(0, 0);
            }
            return;
        }

        if (combos->state == COMBOS_RESTART)
        {

            combos->statePre = combos->state;
            vehicleCmdReset(combos);
            combos->state = COMBOS_RUN;
        }

        if (combos->state != COMBOS_RUN)
        {
            // callProheadingstartupdate();
            // usartPrintf("！COMBOS_RUN\n");
            return;
        }

        if (combos->executedTimes < combos->executeTimes) //指令循环不够
        {
            //动力输出
            if (combos->movedTime < combos->cmd[combos->CurrentCmd].moveTime) //行驶时间没到，输出
            {
                combos->movedTime += 10;
                // usartPrintf("turn = %d,\n",combos->cmd[combos->CurrentCmd].turn);
                program_vehicleWheelCtrl(combos->cmd[combos->CurrentCmd].throttle, combos->cmd[combos->CurrentCmd].shitf, combos->cmd[combos->CurrentCmd].turn);
            }
            if (combos->movedTime >= combos->cmd[combos->CurrentCmd].moveTime) //行驶时间到了，输出清零
            {
                program_vehicleWheelCtrl(0, 0, 0);
                combos->movedTime = combos->cmd[combos->CurrentCmd].moveTime;
            }
            //火力输出
            if (combos->firedTime < combos->cmd[combos->CurrentCmd].fireTime) //开火时间没到
            {
                combos->firedTime += 10;
                program_gunturretCtrl(combos->cmd[combos->CurrentCmd].turretPitch, combos->cmd[combos->CurrentCmd].fire);
            }
            if (combos->firedTime >= combos->cmd[combos->CurrentCmd].fireTime) //开火时间到了，输出清零
            {
                program_gunturretCtrl(0, 0);
                combos->firedTime = combos->cmd[combos->CurrentCmd].fireTime;
            }

            //输出次数统计
            if (combos->firedTime == combos->cmd[combos->CurrentCmd].fireTime && combos->movedTime == combos->cmd[combos->CurrentCmd].moveTime)
            {
                //动力和火力输出到达设定时间，清空时间记录，跳到下条指令
                combos->firedTime = 0;
                combos->movedTime = 0;
                // callProheadingstartupdate();
                combos->CurrentCmd++;
                if (combos->CurrentCmd >= combos->cmdUsed)
                {
                    //指令循环完，回到第一条指令,循环执行次数加1
                    combos->CurrentCmd = 0;
                    combos->executedTimes++;
                    if (combos->executedTimes >= combos->executeTimes)
                    {
                        combos->state = COMBOS_STOP;
                        program_vehicleWheelCtrl(0, 0, 0);
                        program_gunturretCtrl(0, 0);
                        return;
                    }
                }
            }
        }
    }
}

void vehicelCmdSetState(combos_s* combos, combosState_e newState)
{
    combos->state = newState;
}
combosState_e vehicelCmdGetState(combos_s* combos)
{
    return combos->state;
}
